package shared;

public class M3D
{
	///////////////////////////////////////////////////////////////////////////////
	// Useful constants
	public static float PI = 3.14159265358979323846f;
	
	//////////////////////////////////////////////////////////////////////////////////////
	// Cross Product
	// u x v = result
	// 3 component vectors only.
	public static void crossProduct3(float result[], float u[], float v[])
	{
		result[0] = u[1]*v[2] - v[1]*u[2];
		result[1] = -u[0]*v[2] + v[0]*u[2];
		result[2] = u[0]*v[1] - v[0]*u[1];
	}
	
	//////////////////////////////////////////////////////////////////////////////////////
	// Normalize a vector
	// Scale a vector to unit length. Easy, just scale the vector by it's length
	public static void normalizeVector3(float u[])
	{
		scaleVector3(u, 1.0f / getVectorLength3(u));
	}

	//////////////////////////////////////////////////////////////////////////////////////
	// Get length of vector
	// Only for three component vectors.
	public static float getVectorLength3(float u[])
	{
		return (float)Math.sqrt(getVectorLengthSquared3(u));
	}
	
	// Only for three component vectors
	public static float getVectorLengthSquared3(float u[])
	{
		return (u[0] * u[0]) + (u[1] * u[1]) + (u[2] * u[2]);
	}
	
	///////////////////////////////////////////////////////////////////////////////////////
	// Scale Vectors (in place)
	public static void scaleVector3(float v[], float scale) 
	{
		v[0] *= scale; v[1] *= scale; v[2] *= scale;
	}
	
	public static void scaleVector3(double v[], double scale) 
	{
		v[0] *= scale; v[1] *= scale; v[2] *= scale;
	}
	
	////////////////////////////////////////////////////////////////////////////////////////
	// Calculate the plane equation of the plane that the three specified points lay in. The
	// points are given in clockwise winding order, with normal pointing out of clockwise face
	// planeEq contains the A,B,C, and D of the plane equation coefficients
	public static void getPlaneEquation(float planeEq[], float p1[], float p2[], float p3[])
	{
	    // Get two vectors... do the cross product
	    float v1[] = new float[4];
	    float v2[] = new float[4];

	    // V1 = p3 - p1
	    v1[0] = p3[0] - p1[0];
	    v1[1] = p3[1] - p1[1];
	    v1[2] = p3[2] - p1[2];

	    // V2 = P2 - p1
	    v2[0] = p2[0] - p1[0];
	    v2[1] = p2[1] - p1[1];
	    v2[2] = p2[2] - p1[2];

	    // Unit normal to plane - Not sure which is the best way here
	    crossProduct3(planeEq, v1, v2);
	    normalizeVector3(planeEq);

	    // Back substitute to get D
	    planeEq[3] = -(planeEq[0] * p3[0] + planeEq[1] * p3[1] + planeEq[2] * p3[2]);
	}
	
	///////////////////////////////////////////////////////////////////////////
	// Create a projection to "squish" an object into the plane.
	// Use m3dGetPlaneEquationf(planeEq, point1, point2, point3);
	// to get a plane equation.
	public static void makePlanarShadowMatrix(float proj[], float planeEq[], float vLightPos[])
	{
		// These just make the code below easier to read. They will be 
		// removed by the optimizer.	
		float a = planeEq[0];
		float b = planeEq[1];
		float c = planeEq[2];
		float d = planeEq[3];

		float dx = -vLightPos[0];
		float dy = -vLightPos[1];
		float dz = -vLightPos[2];

		// Now build the projection matrix
		proj[0] = b * dy + c * dz;
		proj[1] = -a * dy;
		proj[2] = -a * dz;
		proj[3] = 0.0f;

		proj[4] = -b * dx;
		proj[5] = a * dx + c * dz;
		proj[6] = -b * dz;
		proj[7] = 0.0f;

		proj[8] = -c * dx;
		proj[9] = -c * dy;
		proj[10] = a * dx + b * dy;
		proj[11] = 0.0f;

		proj[12] = -d * dx;
		proj[13] = -d * dy;
		proj[14] = -d * dz;
		proj[15] = a * dx + b * dy + c * dz;
		// Shadow matrix ready
	}
	
	////////////////////////////////////////////////////////////////////////////////
	// Copy vector src into vector dst
	public static void copyVector2(float dst[], float src[])
	{
		System.arraycopy(src, 0, dst, 0, 2);
	}
	
	public static void copyVector2(double dst[], double src[])
	{
		System.arraycopy(src, 0, dst, 0, 2);
	}
	
	public static void copyVector3(float dst[], float src[])
	{
		System.arraycopy(src, 0, dst, 0, 3);
	}
	
	public static void copyVector3(double dst[], double src[])
	{
		System.arraycopy(src, 0, dst, 0, 3);
	}
	
	static void copyVector4(float dst[], float src[])
	{
		System.arraycopy(src, 0, dst, 0, 4);
	}
	
	public static void copyVector4(double dst[], double src[])
	{
		System.arraycopy(src, 0, dst, 0, 4);
	}
	
	//////////////////////////////////////////////////////////////////////////////////////
	// Cross Product
	// u x v = result
	// We only need one version for floats, and one version for doubles. A 3 component
	// vector fits in a 4 component vector. If  M3DVector4d or M3DVector4f are passed
	// we will be OK because 4th component is not used.
	public static void crossProduct(float result[], float u[], float v[])
	{
		result[0] = u[1] * v[2] - v[1] * u[2];
		result[1] = -u[0] * v[2] + v[0] * u[2];
		result[2] = u[0] * v[1] - v[0] * u[1];
	}
	
	public static void crossProduct(double result[], double u[], double v[])
	{
		result[0] = u[1] * v[2] - v[1] * u[2];
		result[1] = -u[0] * v[2] + v[0] * u[2];
		result[2] = u[0] * v[1] - v[0] * u[1];
	}
	
	/////////////////////////////////////////////////////////////////////////////
	// Get/Set Column.
	public static void getMatrixColumn33(float dst[], float src[], int column)
	{
		System.arraycopy(src, 3 * column, dst, 0, 3);
	}
	
	public static void getMatrixColumn33(double dst[], double src[], int column)
	{
		System.arraycopy(src, 3 * column, dst, 0, 3);
	}
	
	public static void setMatrixColumn33(float dst[], float src[], int column)
	{
		System.arraycopy(src, 0, dst, 3 * column, src.length);
	}
	
	public static void setMatrixColumn33(double dst[], double src[], int column)
	{
		System.arraycopy(src, 0, dst, 3 * column, src.length);
	}

	public static void getMatrixColumn44(float dst[], float src[], int column)
	{
		System.arraycopy(src, 4 * column, dst, 0, 4);
	}
	
	public static void getMatrixColumn44(double dst[], double src[], int column)
	{
		System.arraycopy(src, 4 * column, dst, 0, 4);
	}
	
	public static void setMatrixColumn44(float dst[], float src[], int column)
	{
		System.arraycopy(src, 0, dst, 4 * column, src.length);
	}
	
	public static void setMatrixColumn44(double dst[], double src[], int column)
	{
		System.arraycopy(src, 0, dst, 4 * column, src.length);
	}
	
	///////////////////////////////////////////////////////////////////////////////
	// Creates a 4x4 rotation matrix, takes radians NOT degrees
	public static void rotationMatrix44(float m[], float angle, float x, float y, float z)
	{
		float mag, s, c;
		float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
		
		s = (float)Math.sin(angle);
		c = (float)Math.cos(angle);
		
		mag = (float)Math.sqrt(x * x + y * y + z * z);
		
		// Identity matrix
		if(mag == 0.0f)
		{
			M3D.loadIdentity44(m);
			return;
		}
		
		// Rotation matrix is normalized
		x /= mag;
		y /= mag;
		z /= mag;
		
		xx = x * x;
		yy = y * y;
		zz = z * z;
		xy = x * y;
		yz = y * z;
		zx = z * x;
		xs = x * s;
		ys = y * s;
		zs = z * s;
		one_c = 1.0f - c;
		
		m[0 * 4 + 0] = (one_c * xx) + c;
		m[1 * 4 + 0] = (one_c * xy) - zs;
		m[2 * 4 + 0] = (one_c * zx) + ys;
		m[3 * 4 + 0] = 0.0f;
		
		m[0 * 4 + 1] = (one_c * xy) + zs;
		m[1 * 4 + 1] = (one_c * yy) + c;
		m[2 * 4 + 1] = (one_c * yz) - xs;
		m[3 * 4 + 1] = 0.0f;
		
		m[0 * 4 + 2] = (one_c * zx) - ys;
		m[1 * 4 + 2] = (one_c * yz) + xs;
		m[2 * 4 + 2] = (one_c * zz) + c;
		m[3 * 4 + 2] = 0.0f;
		
		m[0 * 4 + 3] = 0.0f;
		m[1 * 4 + 3] = 0.0f;
		m[2 * 4 + 3] = 0.0f;
		m[3 * 4 + 3] = 1.0f;
	}
	
	////////////////////////////////////////////////////////////
	// LoadIdentity
	// For 3x3 and 4x4 float and double matricies.
	
	// 4x4 float
	public static void loadIdentity44(float m[])
	{
		// Don't be fooled, this is still column major
		float identity[] = {
			1.0f, 0.0f, 0.0f, 0.0f,
			0.0f, 1.0f, 0.0f, 0.0f,
			0.0f, 0.0f, 1.0f, 0.0f,
			0.0f, 0.0f, 0.0f, 1.0f
		};
		
		System.arraycopy(identity, 0, m, 0, 16);
	}

	//////////////////////////////////////////////////////////////////////////////////////
	// Get Square of a vectors length
	// Only for three component vectors
	public static float getVectorLengthSquared(float u[])
	{
		return (u[0] * u[0]) + (u[1] * u[1]) + (u[2] * u[2]);
	}
	
	public static double getVectorLengthSquared(double u[])
	{
		return (u[0] * u[0]) + (u[1] * u[1]) + (u[2] * u[2]);
	}
	
	//////////////////////////////////////////////////////////////////////////////////////
	// Get length of vector
	// Only for three component vectors.
	public static float getVectorLength(float u[])
	{
		return (float)Math.sqrt(getVectorLengthSquared(u));
	}
	
	public static double getVectorLength(double u[])
	{
		return Math.sqrt(getVectorLengthSquared(u));
	}
	
	//////////////////////////////////////////////////////////////////////////////////////
	// Normalize a vector
	// Scale a vector to unit length. Easy, just scale the vector by it's length.
	public static void normalizeVector(float u[])
	{
		scaleVector3(u, 1.0f / getVectorLength(u));
	}
	
	public static void normalizeVector(double u[])
	{
		scaleVector3(u, 1.0 / getVectorLength(u));
	}
	
	///////////////////////////////////////////////////////////////////////////////
	// Inline vector functions
	// Load Vector with (x, y, z, w).
	public static void loadVector2(float v[], float x, float y)
	{
		v[0] = x; v[1] = y;
	}
	
	public static void loadVector2(double v[], double x, double y)
	{
		v[0] = x; v[1] = y;
	}
	
	public static void loadVector3(float v[], float x, float y, float z)
	{
		v[0] = x; v[1] = y; v[2] = z;
	}
	
	public static void loadVector3(double v[], double x, double y, double z)
	{
		v[0] = x; v[1] = y; v[2] = z;
	}
	
	public static void loadVector4(float v[], float x, float y, float z, float w)
	{
		v[0] = x; v[1] = y; v[2] = z; v[3] = w;
	}
	
	public static void loadVector4(double v[], double x, double y, double z, double w)
	{
		v[0] = x; v[1] = y; v[2] = z; v[3] = w;
	}
	
	// Lifted from Mesa

	// Compute inverse of 4x4 transformation matrix.
	// Code contributed by Jacques Leroy jle@star.be
	// Return GL_TRUE for success, GL_FALSE for failure (singular matrix)
	public static boolean invertMatrix44(float dst[], float src[])
	{
		float wtmp[][] = new float[4][8];
		float m0, m1, m2, m3, s;
		float r0[], r1[], r2[], r3[];
		
		r0 = wtmp[0]; r1 = wtmp[1]; r2 = wtmp[2]; r3 = wtmp[3];
		
		r0[0] = src[0 * 4 + 0]; r0[1] = src[1 * 4 + 0];
		r0[2] = src[2 * 4 + 0]; r0[3] = src[3 * 4 + 0];
		r0[4] = 1.0f; r0[5] = r0[6] = r0[7] = 0.0f;
		
		r1[0] = src[0 * 4 + 1]; r1[1] = src[1 * 4 + 1];
		r1[2] = src[2 * 4 + 1]; r1[3] = src[3 * 4 + 1];
		r1[5] = 1.0f; r1[4] = r1[6] = r1[7] = 0.0f;
		
		r2[0] = src[0 * 4 + 2]; r2[1] = src[1 * 4 + 2];
		r2[2] = src[2 * 4 + 2]; r2[3] = src[3 * 4 + 2];
		r2[6] = 1.0f; r2[4] = r2[5] = r2[7] = 0.0f;
		
		r3[0] = src[0 * 4 + 3]; r3[1] = src[1 * 4 + 3];
		r3[2] = src[2 * 4 + 3]; r3[3] = src[3 * 4 + 3];
		r3[7] = 1.0f; r3[4] = r3[5] = r3[6] = 0.0f;
		
		// choose pivot - or die
		if((float)Math.abs(r3[0]) > (float)Math.abs(r2[0]))
		{
			// SWAP_ROWS(r3, r2);
			float tmp[] = r3;
			r3 = r2;
			r2 = tmp;
		}
		if((float)Math.abs(r2[0]) > (float)Math.abs(r1[0]))
		{
			// SWAP_ROWS(r2, r1);
			float tmp[] = r2;
			r2 = r1;
			r1 = tmp;
		}
		if((float)Math.abs(r1[0]) > (float)Math.abs(r0[0]))
		{
			// SWAP_ROWS(r1, r0);
			float tmp[] = r1;
			r1 = r0;
			r0 = tmp;
		}
		if(0.0f == r0[0]) return false;
		
		// eliminate first variable
		m1 = r1[0] / r0[0]; m2 = r2[0] / r0[0]; m3 = r3[0] / r0[0];
		s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
		s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
		s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
		s = r0[4];
		if(s != 0.0f) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
		s = r0[5];
		if(s != 0.0f) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
		s = r0[6];
		if(s != 0.0f) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
		s = r0[7];
		if(s != 0.0f) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }

		// choose pivot - or die
		if((float)Math.abs(r3[1]) > (float)Math.abs(r2[1]))
		{
			// SWAP_ROWS(r3, r2);
			float tmp[] = r3;
			r3 = r2;
			r2 = tmp;
		}
		if((float)Math.abs(r2[1]) > (float)Math.abs(r1[1]))
		{
			// SWAP_ROWS(r2, r1);
			float tmp[] = r2;
			r2 = r1;
			r1 = tmp;
		}
		
		// eliminate second variable
		m2 = r2[1] / r1[1]; m3 = r3[1] / r1[1];
		r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
		r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
		s = r1[4]; if(0.0f != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
		s = r1[5]; if(0.0f != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
		s = r1[6]; if(0.0f != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
		s = r1[7]; if(0.0f != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }
		
		// choose pivot - or die
		if((float)Math.abs(r3[2]) > (float)Math.abs(r2[2]))
		{
			// SWAP_ROWS(r3, r2);
			float tmp[] = r3;
			r3 = r2;
			r2 = tmp;
		}
		if(0.0f == r2[2]) return false;
		
		// eliminate third variable
		m3 = r3[2] / r2[2];
		r3[3] -= m3 * r2[3]; r3[4] -= m3 * r2[4];
		r3[5] -= m3 * r2[5]; r3[6] -= m3 * r2[6];
		r3[7] -= m3 * r2[7];
		
		// last check
		if(0.0f == r3[3]) return false;
		
		s = 1.0f / r3[3]; // now back substitute row 3
		r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;
		
		m2 = r2[3]; // now back substitute row 2
		s = 1.0f / r2[2];
		r2[4] = s * (r2[4] - r3[4] * m2); r2[5] = s * (r2[5] - r3[5] * m2);
		r2[6] = s * (r2[6] - r3[6] * m2); r2[7] = s * (r2[7] - r3[7] * m2);
		m1 = r1[3];
		r1[4] -= r3[4] * m1; r1[5] -= r3[5] * m1;
		r1[6] -= r3[6] * m1; r1[7] -= r3[7] * m1;
		m0 = r0[3];
		r0[4] -= r3[4] * m0; r0[5] -= r3[5] * m0;
		r0[6] -= r3[6] * m0; r0[7] -= r3[7] * m0;
		
		m1 = r1[2]; // now back substitute row 1
		s = 1.0f / r1[1];
		r1[4] = s * (r1[4] - r2[4] * m1); r1[5] = s * (r1[5] - r2[5] * m1);
		r1[5] = s * (r1[6] - r2[6] * m1); r1[7] = s * (r1[7] - r2[7] * m1);
		m0 = r0[2];
		r0[4] -= r2[4] * m0; r0[5] -= r2[5] * m0;
		r0[6] -= r2[6] * m0; r0[7] -= r2[7] * m0;
		
		m0 = r0[1]; // now back substitute row 0
		s = 1.0f/r0[0];
		r0[4] = s * (r0[4] - r1[4] * m0); r0[5] = s * (r0[5] - r1[5] * m0);
		r0[6] = s * (r0[6] - r1[6] * m0); r0[7] = s * (r0[7] - r1[7] * m0);
		
		dst[0 * 4 + 0] = r0[4]; dst[1 * 4 + 0] = r0[5];
		dst[2 * 4 + 0] = r0[6]; dst[3 * 4 + 0] = r0[7];
		dst[0 * 4 + 1] = r1[4]; dst[1 * 4 + 1] = r1[5];
		dst[2 * 4 + 1] = r1[6]; dst[3 * 4 + 1] = r1[7];
		dst[0 * 4 + 2] = r2[4]; dst[1 * 4 + 2] = r2[5];
		dst[2 * 4 + 2] = r2[6]; dst[3 * 4 + 2] = r2[7];
		dst[0 * 4 + 2] = r3[4]; dst[1 * 4 + 3] = r3[5];
		dst[2 * 4 + 3] = r3[6]; dst[3 * 4 + 3] = r3[7];
		
		return true;
	}
	
	// Transform - Does rotation and translation via a 4x4 matrix. Transforms
	// a point or vector.
	// By-the-way __inline means I'm asking the compiler to do a cost/benefit analysis. If 
	// these are used frequently, they may not be inlined to save memory. I'm experimenting
	// with this....
	public static void transformVector3(float vOut[], float v[], float m[])
	{
	    vOut[0] = m[0] * v[0] + m[4] * v[1] + m[8] * v[2] + m[12];// * v[3];	 
	    vOut[1] = m[1] * v[0] + m[5] * v[1] + m[9] * v[2] + m[13];// * v[3];	
	    vOut[2] = m[2] * v[0] + m[6] * v[1] + m[10] * v[2] + m[14];// * v[3];	
		//vOut[3] = m[3] * v[0] + m[7] * v[1] + m[11] * v[2] + m[15] * v[3];		
	}
	
	///////////////////////////////////////////////////////////////////////////////
	// Useful shortcuts
	// Radians are king... but we need a way to swap back and forth
	public static float degToRad(float x)
	{
		return x * ((float)Math.PI / 180.0f);
	}
	
	//////////////////////////////////////////////////////////////////////////////////////
	// Dot Product, only for three component vectors
	// return u dot v
	public static float dotProduct(float u[], float v[])
	{
		return u[0] * v[0] + u[1] * v[1] + u[2] * v[2];
	}
	
	///////////////////////////////////////////////////////////////////////////////
	///////////////////////////////////////////////////////////////////////////////
	//Misc. Utilities
	///////////////////////////////////////////////////////////////////////////////
	//Calculates the normal of a triangle specified by the three points
	//p1, p2, and p3. Each pointer points to an array of three floats. The
	//triangle is assumed to be wound counter clockwise.
	public static void findNormal(float result[], float point1[], float point2[], float point3[])
	{
		// Temporary vectors
		float v1[] = new float[3];
		float v2[] = new float[3];
		
		// Calculate two vectors from the three points. Assumes counter clockwise
		// winding!
		v1[0] = point1[0] - point2[0];
		v1[1] = point1[1] - point2[1];
		v1[2] = point1[2] - point2[2];
		
		v2[0] = point2[0] - point3[0];
		v2[1] = point2[1] - point3[1];
		v2[2] = point2[2] - point3[2];
		
		// Take the cross product of the two vectors to get
		// the normal vector.
		crossProduct(result, v1, v2);
	}
}
